﻿////////////////////////////////////////////////////////////////
///
/// File: BodyEventArgs.cs
/// Author: Lucio Davide Spano
/// Last update: 27 July 2012
/// Description: This class checks whether if a hand is open or 
///              not, using the Kinect depth camera. N.B. It is 
///              only a proof of concept, it cannot be considered
///              realiable. 
/// To do: 
/// 
/// The code is based on the KinectDepthViewer sample by Clint Rutkas, 
/// Brian Peek, and Dan Fernandez and on the HandGestureRecognition 
/// using EmguCV project by Luca Del Tongo
/// ------------------------------------------------------------
/// Copyright (c) 2012 Lucio Davide Spano 
///                     (lucio.davide.spano@isti.cnr.it)
/// 
/// The use and distribution terms for this software are 
/// contained in the file named license.txt, which can be found 
/// in the root of this distribution.
/// By using this software in any fashion, you are agreeing to 
/// be bound by the terms of this license.
///
/// You must not remove this notice, or any other, from this
/// software.
/// ------------------------------------------------------------
////////////////////////////////////////////////////////////////

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Emgu.CV;
using Emgu.CV.Structure;
using System.Runtime.InteropServices;
using Microsoft.Kinect;
using System.Drawing;


/// Please Note that you have to add a reference to the EgmuCV dll library, which is 
/// not distributed in this package 

namespace BodyGestIT
{
    /// <summary>
    /// This class checks wheter the hands of the tracked skeletons can be 
    /// considered open or not
    /// 
    /// The code is based on the KinectDepthViewer sample by Clint Rutkas, Brian Peek, and Dan Fernandez
    /// and on the HandGestureRecognition using EmguCV project by Luca Del Tongo
    /// </summary>
    class HandRecognizer
    {
        /// <summary>
        /// Maximum number of trackable skeletons
        /// </summary>
        private const int skeletonCount = 6;

        /// <summary>
        /// side lenght of the hand bitmap, in pixel
        /// </summary>
        private const int L = 100;

        /// <summary>
        /// frames to consider for changing the hand state from closed to
        /// open.
        /// </summary>
        private const int N = 10;

        /// <summary>
        /// max depth value returned by the depth sensor
        /// </summary>
        private const float MaxDepthDistance = 4000;
        /// <summary>
        /// min depth value returned by the depth sensor
        /// </summary>
        private const float MinDepthDistance = 800;
        private const float MaxDepthDistanceOffset = MaxDepthDistance - MinDepthDistance;

        /// <summary>
        /// hand bitmap convex hull
        /// </summary>
        private Seq<System.Drawing.Point> hull;
        /// <summary>
        /// hand bitmap filtered convex hull
        /// </summary>
        private Seq<System.Drawing.Point> filteredHull;
        /// <summary>
        /// Convexity defects on the hand bitmap
        /// </summary>
        private Seq<MCvConvexityDefect> defects;
        private MCvConvexityDefect[] defectArray;
        private MCvBox2D box;

        /// <summary>
        /// maintains the state of the hands for the different skeletons
        /// true if the hand is opened, false otherwise
        /// </summary>
        private bool[] opened = new bool[2 * skeletonCount];

        /// <summary>
        /// accumulates the changes in the hand state (opened or not)
        /// for smoothing the final result
        /// </summary>
        private int[] accumulator = new int[2 * skeletonCount];

        /// <summary>
        /// maintains the current hand positions for all skeletons
        /// </summary>
        private DepthImagePoint[] handPoints = new DepthImagePoint[2 * skeletonCount];

        /// <summary>
        /// depth of the pixel of the current hand point, detected using skeleton tracking
        /// </summary>
        private int referenceDepth = 0;
        /// <summary>
        /// color of the pixel of the current hand point, detected using skeleton tracking
        /// </summary>
        private int referenceCol = 0;
        /// <summary>
        /// current index in the pixelData array
        /// </summary>
        private int pxDataIndex = 0;
        /// <summary>
        /// current pixel depth
        /// </summary>
        private int depth = -1;
        /// <summary>
        /// current pixel color
        /// </summary>
        private int col = 0;
        /// <summary>
        /// index of the current player
        /// </summary>
        private int playerIndex = 0;
        /// <summary>
        /// depth threshold for considering the current pixel in the hand
        /// </summary>
        private int threshold = 100;

        /// <summary>
        /// raw data from the current depth frame
        /// </summary>
        private short[] pixelData;
        /// <summary>
        /// counted fingers. It is not really reliable... the magic number is 3
        /// if fingerNum is greater than or equals to 4, the hand is open
        /// otherwise is closed
        /// </summary>
        int fingerNum;

        /// <summary>
        /// The current bitmap to analyse
        /// </summary>
        Emgu.CV.Image<Emgu.CV.Structure.Gray, Byte> bitmap = new Emgu.CV.Image<Emgu.CV.Structure.Gray, Byte>(L, L);

        /// <summary>
        ///  white color in EmguCV for grayscale bitmaps
        /// </summary>
        Emgu.CV.Structure.Gray white = new Emgu.CV.Structure.Gray(255.0);
        /// <summary>
        ///  black color in EmguCV for grayscale bitmaps
        /// </summary>
        Emgu.CV.Structure.Gray black = new Emgu.CV.Structure.Gray(0.0);

        public void ProcessDepthImageFrame(DepthImageFrame depthFrame)
        {
            if (depthFrame != null)
            {
                if (pixelData == null)
                {
                    pixelData = new short[depthFrame.PixelDataLength];
                }

                depthFrame.CopyPixelDataTo(pixelData);

                for (int handIndex = 0; handIndex < handPoints.Length; handIndex++)
                {
                    DepthImagePoint dip = handPoints[handIndex];
                    if (dip.X >= 0 && dip.Y >= 0)
                    {
                        referenceDepth = GetDepth(pixelData[Math.Min((int)(dip.Y) * depthFrame.Width + (int)(dip.X), pixelData.Length - 1)]);
                        referenceCol = CalculateIntensityFromDepth(referenceDepth);
                        pxDataIndex = 0;
                        depth = -1;
                        col = 0;
                        playerIndex = 0;

                        for (int i = 0; i < L * L; i++)
                        {

                            pxDataIndex = (i / L + Math.Max((int)(dip.Y - L / 2), 0)) * depthFrame.Width +
                                (i % L) + Math.Max((int)(dip.X - L / 2), 0);
                            if (pxDataIndex < pixelData.Length)
                            {
                                depth = GetDepth(pixelData[pxDataIndex]);
                                col = CalculateIntensityFromDepth(depth);
                                playerIndex = GetPlayerIndex(pixelData[pxDataIndex]);


                                // how to get the hand data:
                                // filter when depth < 0 or the current pixel has not a player index
                                // and then use a threshold for the current depth value, compared 
                                // with the hand center. If it is in the interval [referenceDepth - threshold, referenceDepth + threshold]
                                // then accept the pixel as an hand one

                                // the current threshold is 100 (found empirically)

                                if (depth > 0 && playerIndex > 0 && depth <= referenceDepth + threshold && depth >= referenceDepth - threshold)
                                {
                                    bitmap[(i / L), (i % L)] = white;
                                }
                                else
                                {
                                    bitmap[(i / L), (i % L)] = black;
                                }

                            }
                            else
                            {
                                bitmap[(i / L), (i % L)] = black;
                            }
                        }


                        // ok, got the bitmap, calculate the contour and convex hull

                        ExtractContourAndHull(bitmap);

                        // calculate defects
                        CountFingers();


                        // now if the finger num is greater than or equals to 3, we consider
                        // the hand open, otherwise we consider it close for this frame
                        // the for changing the hand state, we want that the hand 
                        // maintains stable its status for more than N frames

                        if (fingerNum >= 3)
                        {
                            if (accumulator[handIndex] < N)
                            {
                                accumulator[handIndex]++;
                            }
                            else
                            {
                                opened[handIndex] = true;
                            }
                        }
                        else
                        {
                            if (accumulator[handIndex] > 0)
                            {
                                accumulator[handIndex]--;
                            }
                            else
                            {
                                opened[handIndex] = false;
                            }
                        }

                    }
                }
            }
        }

        public void ResetHandPoints()
        {
            for (int i = 0; i < handPoints.Length; i++)
            {
                handPoints[i].X = -1;
                handPoints[i].Y = -1;

            }
        }

        public void SetHandPoint(int playerIndex, int x, int y, JointType hand)
        {
            if (!(hand == JointType.HandLeft || hand == JointType.HandRight))
            {
                return;
            }
            int realIndex = GetRealIndex(playerIndex);
            if (hand == JointType.HandRight)
            {
                realIndex++;
            }
            if (realIndex > 0 && realIndex < handPoints.Length)
            {
                handPoints[realIndex].X = x;
                handPoints[realIndex].Y = y;
            }

        }

        private static int GetRealIndex(int playerIndex)
        {
            //Console.WriteLine("Real Index: {0}", (playerIndex % skeletonCount) * 2);
            return (playerIndex % skeletonCount) * 2;
        }

        public bool IsOpen(int playerIndex, JointType hand)
        {
            if (!(hand == JointType.HandLeft || hand == JointType.HandRight))
            {
                return false;
            }
            int realIndex = GetRealIndex(playerIndex);
            if (hand == JointType.HandRight)
            {
                realIndex++;
            }
            if (realIndex > 0 && realIndex < opened.Length)
            {
                return opened[realIndex];
            }
            else
            {
                return false;
            }

        }

        private int GetPlayerIndex(short depthPoint)
        {
            return depthPoint & DepthImageFrame.PlayerIndexBitmask;
        }

        private int GetDepth(short depthPoint)
        {
            return depthPoint >> DepthImageFrame.PlayerIndexBitmaskWidth;
        }


        private byte CalculateIntensityFromDepth(int distance)
        {

            return (byte)(255 - (255 * Math.Max(distance - MinDepthDistance, 0) / (MaxDepthDistanceOffset)));
        }

        private void ExtractContourAndHull(Emgu.CV.Image<Emgu.CV.Structure.Gray, Byte> skin)
        {
            using (MemStorage storage = new MemStorage())
            {

                Contour<System.Drawing.Point> contours = skin.FindContours(Emgu.CV.CvEnum.CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, Emgu.CV.CvEnum.RETR_TYPE.CV_RETR_LIST, storage);
                Contour<System.Drawing.Point> biggestContour = null;

                Double Result1 = 0;
                Double Result2 = 0;
                while (contours != null)
                {
                    Result1 = contours.Area;
                    if (Result1 > Result2)
                    {
                        Result2 = Result1;
                        biggestContour = contours;
                    }
                    contours = contours.HNext;
                }

                if (biggestContour != null)
                {
                    Contour<System.Drawing.Point> currentContour = biggestContour.ApproxPoly(biggestContour.Perimeter * 0.0025, storage);
                    biggestContour = currentContour;


                    hull = biggestContour.GetConvexHull(Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE);
                    box = biggestContour.GetMinAreaRect();
                    PointF[] points = box.GetVertices();

                    System.Drawing.Point[] ps = new System.Drawing.Point[points.Length];
                    for (int i = 0; i < points.Length; i++)
                        ps[i] = new System.Drawing.Point((int)points[i].X, (int)points[i].Y);




                    filteredHull = new Seq<System.Drawing.Point>(storage);
                    for (int i = 0; i < hull.Total; i++)
                    {
                        if (Math.Sqrt(Math.Pow(hull[i].X - hull[i + 1].X, 2) + Math.Pow(hull[i].Y - hull[i + 1].Y, 2)) > box.size.Width / 10)
                        {
                            filteredHull.Push(hull[i]);
                        }
                    }

                    defects = biggestContour.GetConvexityDefacts(storage, Emgu.CV.CvEnum.ORIENTATION.CV_CLOCKWISE);

                    defectArray = defects.ToArray();
                }
            }
        }

        private void CountFingers()
        {
            fingerNum = 0;

            if (defects == null)
            {
                return;
            }
            #region defects drawing
            for (int i = 0; i < defects.Total; i++)
            {
                PointF startPoint = new PointF((float)defectArray[i].StartPoint.X,
                                                (float)defectArray[i].StartPoint.Y);

                PointF depthPoint = new PointF((float)defectArray[i].DepthPoint.X,
                                                (float)defectArray[i].DepthPoint.Y);

                PointF endPoint = new PointF((float)defectArray[i].EndPoint.X,
                                                (float)defectArray[i].EndPoint.Y);

                LineSegment2D startDepthLine = new LineSegment2D(defectArray[i].StartPoint, defectArray[i].DepthPoint);

                LineSegment2D depthEndLine = new LineSegment2D(defectArray[i].DepthPoint, defectArray[i].EndPoint);

                CircleF startCircle = new CircleF(startPoint, 5f);

                CircleF depthCircle = new CircleF(depthPoint, 5f);

                CircleF endCircle = new CircleF(endPoint, 5f);

                //Custom heuristic based on some experiment, double check it before use
                if ((startCircle.Center.Y < box.center.Y || depthCircle.Center.Y < box.center.Y) && (startCircle.Center.Y < depthCircle.Center.Y) && (Math.Sqrt(Math.Pow(startCircle.Center.X - depthCircle.Center.X, 2) + Math.Pow(startCircle.Center.Y - depthCircle.Center.Y, 2)) > box.size.Height / 6.5))
                {
                    fingerNum++;
                }


            }
            #endregion

        }
    }
}
